from spirems import Publisher, def_msg, cvimg2sms
import time
import numpy as np
import cv2

pub = Publisher('/topic/share_mem', 'memory_msgs::RawImage')
# img = np.ones((480, 640, 3), dtype=np.uint8) * 127
cap = cv2.VideoCapture('/home/jario/2024-11-12 13-12-58.mkv')
while True:
    try:
        ret, img = cap.read()
        if not ret:
            cap.set(cv2.CAP_PROP_POS_FRAMES, 0)
            continue
        img = cv2.resize(img, (1920, 1080))
        msg = pub.cvimg2sms_mem(img)
        pub.publish(msg)
        time.sleep(0.02)
    except KeyboardInterrupt:
        pub.kill()
        break

